#include "industrial_robot_control/safety/safety_monitor.h"
#include "yaml-cpp/yaml.h"
#include <iostream>
#include <fstream>
#include <stdexcept>
#include <mutex>

namespace industrial_robot_control {
namespace safety {

SafetyMonitor::SafetyMonitor(ethercat::Master& ethercat_master) : ethercat_master_(ethercat_master) {
}

bool SafetyMonitor::initialize(const std::string& config_path) {
    try {
        YAML::Node config = YAML::LoadFile(config_path);
        
        // 加载急停信号配置
        if (config["emergency_stop_signals"]) {
            for (const auto& node : config["emergency_stop_signals"]) {
                EmergencyStopSignal signal;
                signal.name = node["name"].as<std::string>();
                signal.slave_idx = node["slave_idx"].as<uint16_t>();
                signal.sdo_index = node["sdo_index"].as<uint16_t>();
                signal.sdo_subindex = node["sdo_subindex"].as<uint8_t>();
                signal.active = false;
                
                estop_signals_.push_back(signal);
                std::cout << "加载急停信号配置: " << signal.name << std::endl;
            }
        }
        
        // 加载安全区域配置
        if (config["safety_zones"]) {
            for (const auto& node : config["safety_zones"]) {
                SafetyZone zone;
                zone.name = node["name"].as<std::string>();
                zone.is_forbidden = node["is_forbidden"].as<bool>(true);
                
                // 加载边界点
                if (node["boundary"]) {
                    for (const auto& point_node : node["boundary"]) {
                        Eigen::Vector3d point;
                        point.x() = point_node[0].as<double>();
                        point.y() = point_node[1].as<double>();
                        point.z() = point_node[2].as<double>();
                        zone.boundary.push_back(point);
                    }
                }
                
                safety_zones_.push_back(zone);
                std::cout << "加载安全区域配置: " << zone.name 
                          << ", 边界点数量: " << zone.boundary.size() << std::endl;
            }
        }
        
        std::cout << "安全监控系统初始化成功" << std::endl;
        return true;
    } catch (const std::exception& e) {
        std::cerr << "安全监控系统初始化错误: " << e.what() << std::endl;
        setSafetyLevel(SafetyLevel::SAFE_STOP, "安全配置加载失败: " + std::string(e.what()));
        return false;
    }
}

void SafetyMonitor::checkSafety() {
    // 检查EtherCAT主站是否正常运行
    if (!ethercat_master_.isRunning()) {
        setSafetyLevel(SafetyLevel::SAFE_STOP, "EtherCAT主站未运行");
        return;
    }
    
    // 处理急停信号
    processEmergencyStopSignals();
    
    // 如果已经处于急停状态，不需要继续检查
    if (current_level_ == SafetyLevel::EMERGENCY_STOP) {
        return;
    }
    
    // 检查安全区域
    checkSafetyZones();
    
    // 这里可以添加更多安全检查...
}

bool SafetyMonitor::triggerEmergencyStop(const std::string& reason) {
    setSafetyLevel(SafetyLevel::EMERGENCY_STOP, "紧急停止: " + reason);
    return true;
}

bool SafetyMonitor::resetEmergencyStop() {
    // 只有在所有急停信号都已解除时才能重置
    bool all_clear = true;
    for (const auto& signal : estop_signals_) {
        if (signal.active) {
            all_clear = false;
            break;
        }
    }
    
    if (all_clear) {
        setSafetyLevel(SafetyLevel::NORMAL, "紧急停止已重置");
        return true;
    } else {
        setSafetyLevel(SafetyLevel::EMERGENCY_STOP, "无法重置紧急停止，仍有激活的急停信号");
        return false;
    }
}

bool SafetyMonitor::isInSafeZone(const Eigen::Vector3d& position) const {
    // 检查是否在任何禁止区域内
    for (const auto& zone : safety_zones_) {
        if (zone.is_forbidden && !zone.boundary.empty() && 
            isPointInPolygon(position, zone.boundary)) {
            return false;
        }
    }
    return true;
}

bool SafetyMonitor::checkCollision(const Eigen::Vector3d& current_pos, const Eigen::Vector3d& next_pos) {
    // 简化的碰撞检测实现
    // 实际应用中应使用更复杂的算法
    
    // 检查是否在安全区域内
    if (!isInSafeZone(next_pos)) {
        return true;
    }
    
    // 可以添加与其他物体的碰撞检测...
    
    return false;
}

void SafetyMonitor::processEmergencyStopSignals() {
    // 读取所有急停信号状态
    for (auto& signal : estop_signals_) {
        bool current_state = false;
        
        // 从EtherCAT从站读取急停信号状态
        if (ethercat_master_.readSlaveData(signal.slave_idx, signal.sdo_index, 
                                         signal.sdo_subindex, &current_state, sizeof(current_state))) {
            // 如果急停信号被激活
            if (current_state && !signal.active) {
                signal.active = true;
                signal.last_triggered = std::chrono::system_clock::now();
                triggerEmergencyStop("急停信号激活: " + signal.name);
            }
            // 如果急停信号被解除
            else if (!current_state && signal.active) {
                signal.active = false;
                std::cout << "急停信号已解除: " << signal.name << std::endl;
            }
        } else {
            std::cerr << "读取急停信号失败: " << signal.name << std::endl;
            setSafetyLevel(SafetyLevel::WARNING, "读取急停信号失败: " + signal.name);
        }
    }
}

void SafetyMonitor::checkSafetyZones() {
    // 在实际应用中，这里应该获取机器人末端执行器的当前位置
    // 这里使用一个示例位置进行演示
    Eigen::Vector3d current_position(0.0, 0.0, 0.5);
    
    if (!isInSafeZone(current_position)) {
        setSafetyLevel(SafetyLevel::SAFE_STOP, "机器人进入禁止区域");
    } else if (current_level_ > SafetyLevel::NORMAL) {
        // 如果之前有安全问题但现在已解决，恢复正常状态
        setSafetyLevel(SafetyLevel::NORMAL, "安全状态已恢复正常");
    }
}

void SafetyMonitor::setSafetyLevel(SafetyLevel level, const std::string& error) {
    std::lock_guard<std::mutex> lock(error_mutex_);
    
    if (level != current_level_) {
        current_level_ = level;
        last_error_ = error;
        
        std::cout << "安全级别变更为: " << static_cast<int>(level) << ", 原因: " << error << std::endl;
    } else {
        last_error_ = error;
    }
}

bool SafetyMonitor::isPointInPolygon(const Eigen::Vector3d& point, const std::vector<Eigen::Vector3d>& polygon) const {
    // 简化的2D多边形点包含检测 (忽略Z轴)
    // 实际应用中可能需要3D碰撞检测
    
    int n = polygon.size();
    bool inside = false;
    
    for (int i = 0, j = n - 1; i < n; j = i++) {
        const auto& p1 = polygon[i];
        const auto& p2 = polygon[j];
        
        // 检查点是否在边的Y范围内
        if (((p1.y() > point.y()) != (p2.y() > point.y()))) {
            // 计算X交点
            double x_intersect = ( (point.y() - p1.y()) * (p2.x() - p1.x()) ) / (p2.y() - p1.y()) + p1.x();
            
            // 如果点的X小于交点X，则在内部
            if (point.x() < x_intersect) {
                inside = !inside;
            }
        }
    }
    
    return inside;
}

} // namespace safety
} // namespace industrial_robot_control
